#include "sdkconfig.h"
.equ MODE_SVC, 0x13

/* allocatable and executable */
.section .vector_table, "ax"

.balign 2048
.global system_vectors
system_vectors:
    b RST_Handler
    b UDF_Handler     /* 0x4  Undefined Instruction */
    b SWI_Handler     /* 0x8  Software Interrupt */
    b PRF_Handler     /* 0xC  Prefetch Abort */
    b DAT_Handler     /* 0x10 Data Abort */
    b .               /* 0x14 Reserved */
    b IRQ_Handler     /* 0x18 IRQ */
    b FIQ_Handler     /* 0x1C FIQ */    

.global Abort_Exception
Abort_Exception:
    swi 0xFF

.macro PushSVCRegisters
    sub     sp, sp, #17 * 4         @/* Sizeof(struct rt_hw_exp_stack)  */
    stmia   sp, {r0 - r12}          @/* Calling r0-r12                  */
    mov     r0, sp
    mrs     r6, spsr                @/* Save CPSR                       */
    str     lr, [r0, #15*4]         @/* Push PC                         */
    str     r6, [r0, #16*4]         @/* Push CPSR                       */
    cps     #MODE_SVC
    str     sp, [r0, #13*4]         @/* Save calling SP                 */
    str     lr, [r0, #14*4]         @/* Save calling PC                 */
.endm

.align 4
.arm
.weak IRQ_Handler
.type IRQ_Handler, %function
@.global IRQ_Handler
IRQ_Handler: /* 0x18 IRQ */
    push    {lr}                         /* Save return address+4                                */
    push    {r0-r3, r12}                 /* Push caller save registers                           */

    mrs     r0, spsr                     /* Save SPRS to allow interrupt reentry                 */
    push    {r0}
    
    /* Get GIC base address  MRC   P15, 4, r1, C15, C0, 0  */
    //<! get iar 
    mrc p15, 0, r0, c12, c12, 0
    isb

    push    {r0, r1}

    cps     #0x13                        /* Change to Supervisor mode to allow interrupt reentry */
    
    push    {lr}                         /* Save Supervisor lr                                   */
   
    ldr     r2, =SystemIrqHandler
    blx     r2                           /*  b irq_debug Call SystemIrqHandler with param GCC                 */

    pop     {lr}

    cps     #0x12                        /* Back to IRQ mode                                     */

    pop     {r0, r1}

    pop     {r0}
    msr     spsr_cxsf, r0

    pop     {r0-r3, r12}
    pop     {lr}
    subs    pc, lr, #4    
    .size IRQ_Handler, . - IRQ_Handler

.align 2
.arm
.weak FIQ_Handler
.type FIQ_Handler, %function
.global FIQ_Handler
FIQ_Handler: /* 0x18 IRQ */
    bl FiqDebug
    b  Abort_Exception
    .size FIQ_Handler, . - FIQ_Handler

.align 2
.arm
.weak UDF_Handler
.type UDF_Handler, %function
.global UDF_Handler
UDF_Handler: /* 0x4  Undefined Instruction */
    bl UdfDebug
    b  Abort_Exception
    .size UDF_Handler, . - UDF_Handler

.align 2
.arm
.weak SWI_Handler
.type SWI_Handler, %function
.global SWI_Handler
SWI_Handler: /* 0x8  Software Interrupt */
    bl SwiDebug
    b  Abort_Exception
    .size SWI_Handler, . - SWI_Handler

.align 2
.arm
.weak PRF_Handler
.type PRF_Handler, %function
.global PRF_Handler
PRF_Handler:  /* 0xC  Prefetch Abort */
    PushSVCRegisters
    bl  PrfTrap
    b   Abort_Exception
    .size PRF_Handler, . - PRF_Handler

.align 2
.arm
.weak DAT_Handler
.type DAT_Handler, %function
.global DAT_Handler
DAT_Handler: /* 0x10 Data Abort */
    bl DatDebug
    b  Abort_Exception
    .size DAT_Handler, . - DAT_Handler